/**
 * @file serial_data.h
 * @author 范文宇 (2643660853@qq.com)
 * @brief
 * @version 1.0
 * @date 2022-12-02
 *
 * @copyright Copyright (c) 2022
 *
 */
#pragma once

#include "protocol.h"
#include "serial_port.h"
// 接收协议
struct ReceiveData
{
    // imu数据
    uint8_t mode_L_hip;   // L_hip_motor current mode
    uint8_t mode_L_hip2;  // L_hip2_motor current mode
    uint8_t mode_L_thign; // L_thign_motor current mode
    uint8_t mode_L_calf;  // L_calf_motor current mode
    uint8_t mode_L_toe;   // L_toe_motor current mode

    uint8_t mode_R_hip;   // R_hip_motor current mode
    uint8_t mode_R_hip2;  // R_hip2_motor current mode
    uint8_t mode_R_thign; // R_thign_motor current mode
    uint8_t mode_R_calf;  // R_calf_motor current mode
    uint8_t mode_R_toe;   // R_toe_motor current mode

    // imu数据
    float position_x;
    float position_y;
    float position_z;
    float quaternion_w;
    float quaternion_x;
    float quaternion_y;
    float quaternion_z;

    float gyroscope_x;
    float gyroscope_y;
    float gyroscope_z;

    float accelerometer_x;
    float accelerometer_y;
    float accelerometer_z;

    float q_L_hip;      // L_hip_motor current position（rad）
    float dq_L_hip;     // L_hip_motor current speed（rad/s）
    float tauEst_L_hip; // L_hip_current estimated output torque（N*m）

    float q_L_hip2;      // L_hip2_motor current position（rad）
    float dq_L_hip2;     // L_hip2_motor current speed（rad/s）
    float tauEst_L_hip2; // L_hip2_current estimated output torque（N*m）

    float q_L_thign;      // L_thign_motor current position（rad）
    float dq_L_thign;     // L_thign_motor current speed（rad/s）
    float tauEst_L_thign; // L_thign_current estimated output torque（N*m）

    float q_L_calf;      // L_calf_motor current position（rad）
    float dq_L_calf;     // L_calf_motor current speed（rad/s）
    float tauEst_L_calf; // L_calf_current estimated output torque（N*m）

    float q_L_toe;      // L_toe_motor current position（rad）
    float dq_L_toe;     // L_toe_motor current speed（rad/s）
    float tauEst_L_toe; // L_toe_current estimated output torque（N*m）

    // ##########################LR分割线###########################
    float q_R_hip;      // R_hip_motor current position（rad）
    float dq_R_hip;     // R_hip_motor current speed（rad/s）
    float tauEst_R_hip; // R_hip_current estimated output torque（N*m）

    float q_R_hip2;      // R_hip2_motor current position（rad）
    float dq_R_hip2;     // R_hip2_motor current speed（rad/s）
    float tauEst_R_hip2; // R_hip2_current estimated output torque（N*m）

    float q_R_thign;      // R_thign_motor current position（rad）
    float dq_R_thign;     // R_thign_motor current speed（rad/s）
    float tauEst_R_thign; // R_thign_current estimated output torque（N*m）

    float q_R_calf;      // R_calf_motor current position（rad）
    float dq_R_calf;     // R_calf_motor current speed（rad/s）
    float tauEst_R_calf; // R_calf_current estimated output torque（N*m）

    float q_R_toe;      // R_toe_motor current position（rad）
    float dq_R_toe;     // R_toe_motor current speed（rad/s）
    float tauEst_R_toe; // R_toe_current estimated output torque（N*m）

} __attribute__((packed));

// 发送协议
#pragma pack(1)
struct SendData
{
    uint8_t mode_L_hip;   // L_hip_motor target mode
    uint8_t mode_L_hip2;  // L_hip2_motor target mode
    uint8_t mode_L_thigh; // L_thigh_motor target mode
    uint8_t mode_L_calf;  // L_calf_motor target mode
    uint8_t mode_L_toe;   // L_toe_motor target mode
    uint8_t mode_R_hip;   // R_hip_motor target mode
    uint8_t mode_R_hip2;  // R_hip2_motor target mode
    uint8_t mode_R_thigh; // R_thigh_motor target mode
    uint8_t mode_R_calf;  // R_calf_motor target mode
    uint8_t mode_R_toe;   // R_toe_motor target mode

    float q_L_hip;   // L_hip_motor target position
    float dq_L_hip;  // L_hip_motor target velocity
    float tau_L_hip; // L_hip_motor target torque
    float Kp_L_hip;  // L_hip_motor spring stiffness coefficient
    float Kd_L_hip;  // L_hip_motor damper coefficient
    // uint32[3] reserve_L_hip    // L_hip_motor target torque

    float q_L_hip2;   // L_hip2_motor target position
    float dq_L_hip2;  // L_hip2_motor target velocity
    float tau_L_hip2; // L_hip2_motor target torque
    float Kp_L_hip2;  // L_hip2_motor spring stiffness coefficient
    float Kd_L_hip2;  // L_hip2_motor damper coefficient
    // uint32[3] reserve_L_hip2    // L_hip2_motor target torque

    float q_L_thigh;   // L_thigh_motor target position
    float dq_L_thigh;  // L_thigh_motor target velocity
    float tau_L_thigh; // L_thigh_motor target torque
    float Kp_L_thigh;  // L_thigh_motor spring stiffness coefficient
    float Kd_L_thigh;  // L_thigh_motor damper coefficient
    // uint32[3] reserve_L_thigh    // L_thigh motor target torque

    float q_L_calf;   // L_calf_motor target position
    float dq_L_calf;  // L_calf_motor target velocity
    float tau_L_calf; // L_calf_motor target torque
    float Kp_L_calf;  // L_calf_motor spring stiffness coefficient
    float Kd_L_calf;  // L_calf_motor damper coefficient
    // uint32[3] reserve_L_calf    // L_calf motor target torque

    float q_L_toe;   // L_toe_motor target position
    float dq_L_toe;  // L_toe_motor target velocity
    float tau_L_toe; // L_toe_motor target torque
    float Kp_L_toe;  // L_toe_motor spring stiffness coefficient
    float Kd_L_toe;  // L_toe_motor damper coefficient
    /// uint32[3] reserve_L_toe    // L_toe motor target torque

    // ##############################LR分界线#####################################################

    float q_R_hip;   // R_hip_motor target position
    float dq_R_hip;  // R_hip_motor target velocity
    float tau_R_hip; // R_hip_motor target torque
    float Kp_R_hip;  // R_hip_motor spring stiffness coefficient
    float Kd_R_hip;  // R_hip_motor damper coefficient
    // uint32[3] reserve_R_hip    // R_hip_motor target torque

    float q_R_hip2;   // R_hip2_motor target position
    float dq_R_hip2;  // R_hip2_motor target velocity
    float tau_R_hip2; // R_hip2_motor target torque
    float Kp_R_hip2;  // R_hip2_motor spring stiffness coefficient
    float Kd_R_hip2;  // R_hip2_motor damper coefficient
    // uint32[3] reserve_R_hip2    // R_hip2_motor target torque

    float q_R_thigh;   // R_thigh_motor target position
    float dq_R_thigh;  // R_thigh_motor target velocity
    float tau_R_thigh; // R_thigh_motor target torque
    float Kp_R_thigh;  // R_thigh_motor spring stiffness coefficient
    float Kd_R_thigh;  // R_thigh_motor damper coefficient
    // uint32[3] reserve_R_thigh    // R_thigh motor target torque

    float q_R_calf;   // R_calf_motor target position
    float dq_R_calf;  // R_calf_motor target velocity
    float tau_R_calf; // R_calf_motor target torque
    float Kp_R_calf;  // R_calf_motor spring stiffness coefficient
    float Kd_R_calf;  // R_calf_motor damper coefficient
    // uint32[3] reserve_R_calf    # R_calf motor target torque

    float q_R_toe;   // R_toe_motor target position
    float dq_R_toe;  // R_toe_motor target velocity
    float tau_R_toe; // R_toe_motor target torque
    float Kp_R_toe;  // R_toe_motor spring stiffness coefficient
    float Kd_R_toe;  // R_toe_motor damper coefficient

    uint8_t end_flag;
    // uint32[3] reserve_R_toe    # R_toe motor target torque
};
#pragma pack()

// 协议数据包
class SerialData : public protocol
{
private:
    ReceiveData __receive_data; // 视觉接收数据
    SendData __send_data;       // 视觉发送数据
    SerialPort __serial_port;   // 串口
    TransferData __last_data;   // 上一帧的传递数据
    bool __is_init = false;     // 是否初始化
public:
    SerialData() : protocol() {}

    /**
     * @brief 读取串口
     *
     * @param head 头帧
     * @param tail 尾帧
     */
    TransferData read() override;

    /**
     * @brief 写入串口
     *
     * @param transfer_data 传递数据
     */
    void write(const TransferData &transfer_data) override;

    bool isOpened() override;
};
inline SerialData serialdata;
